
#include "Matrix.h"
#include "Calculus.h"
#include "Spline.h"

#include <ctime>
#include <random>

#include <iostream>

using namespace std;
using namespace LinearAlgebra;
using namespace Calculus;
using namespace Mesh;
using namespace UniPolynomial;

void print(Matrix &matrix)
{
    for (int i = 0; i < matrix.row(); i++)
    {
        for (int j = 0; j < matrix.col(); j++)
        {
            cout << matrix(i, j) << " ";
        }
        cout << endl;
    }
    cout << endl;
}
void print(Vector &v)
{
    for (int i = 0; i < v.size(); i++)
    {
        cout << v[i] << " ";
    }
    cout << endl;
}
void print(Polynomial &p)
{
    for (int i = 0; i < p.order(); i++)
        cout << p.coeff(i) << "x^" << i << " ";
    cout << endl;
}
void print(PiecewisePolynomial &pp)
{
    for (int i = 0; i < pp.size() + 1; i++)
        print(pp[i]);
    cout << endl;
}

void test()
{
    Matrix m(3, 3, {1, 4, 7, 2, 5, 8, 3, 6, 10});
    print(m);
    Vector b({12, 15, 19});

    LinearSolver solver;
    Vector x = solver.solve(m, b);
    print(x);

    Matrix Q = m.QR(x);
    print(Q);

    cout << endl;

    Matrix m2(4, 4, {4, -2, 4, 2, -2, 10, -2, -7, 4, -2, 8, 4, 2, -7, 4, 7});
    Matrix m3 = m2.Cholesky();
    print(m3);

    Vector y({8, -1, 14, 6});
    Vector x1 = solver.CG(m2, y);
    print(x1);
}

void test2()
{
    Matrix m(3, 3, {1, 2, -2, 1, 1, 1, 2, 2, 1});
    Vector y({3, 2, 4});
    print(m);

    Matrix im = m.inverse();
    print(im);

    Matrix I = im * m;
    print(I);

    Matrix spm = m;

    LinearSolver solver;
    Vector x = solver.Jacobi(spm, y);
    print(x);
}

void test3()
{
    Matrix m(3, 3, {2, -1, 1, 1, 1, 1, 1, 1, -2});
    Vector y({1, 2, 2});
    print(m);

    Matrix spm = m;

    LinearSolver solver;
    Vector x = solver.SOR(spm, y, 0.8);
    print(x);
}

void test4()
{
    // x^5 - 15*x^4 + 85*x^3 - 225*x^2 + 274*x - 120
    // x^4 - 14*x^3 + 71*x^2 - 154*x + 120
    Polynomial p({120, -154, 71, -14, 1});
    Matrix m = p.Laplace();
    print(m);

    LinearSolver solver;
    Real lambda = solver.power(m);
    cout << lambda << endl;
    cout << p(3) << endl;

    // Matrix Q(4, 4);
    // Matrix A = m.Hessenberg(Q);
    // print(A);
    Matrix A = solver.implicitQR(m);
    print(A);
    cout << p(-1.65355) << endl;
}

void test5()
{
    Matrix m(6, 6, {1.1908, -1.0565, -2.1707, 0.5913, 0, 0.7310, -1.2025, 1.4151, -0.0592, -0.6436, -0.3179, 0.5779, -0.0198, -0.8051, -1.0106, 0.3803, 1.0950, 0.0403, -0.1567, 0.5287, 0.6145, -1.0091, -1.8740, 0.6771, -1.6041, 0.2193, 0.5077, -0.0195, 0.4282, 0.5689, 0.2573, -0.9219, 1.6924, -0.0482, 0.8956, -0.2556});
    print(m);

    Matrix Q(6, 6);
    Matrix H = m.Hessenberg(&Q);
    print(H);
    print(Q);

    int n = 6;
    for (int i = n - 3; i >= 0; i--)
    {
        cout << "A" << endl;
        Vector v(n);
        for (int j = i + 1; j < n; j++)
        {
            v[j] = Q(j, i);
        }
        Real beta = Q(i + 1, i);
        v[i + 1] = 1;
        cout << "A1" << endl;

        Matrix H1(n, n);
        for (int j = 0; j < n; j++)
        {
            H1(j, j) = 1;
        }
        H1 = H1 - v.toMatrix() * v.transpose() * beta;

        H = H1 * H * H1;
    }
    print(H);

    // Matrix m2 = Q * H * Q.transpose();
    // print(m2);

    // LinearSolver solver;
    // Matrix A = solver.implicitQR(m);
    // print(A);
}

void test6()
{
    Polynomial p({2, 3, 4});
    p.push(1);
    p.push(2);
    p.push(3);
    print(p);

    Polynomial p2({5, 6});
    Polynomial p3({1, 2, 3});
    Polynomial p4 = p3.intergral();
    print(p4);
    p4 = p3.derivative();
    print(p4);
}

void test7()
{
    const Real PI = 3.1415926;
    NewtonCotes I(6);

    FuncXX fX = [](Vector x) -> Vector
    {
        return {sin(x[0]) * cos(x[1]), sin(x[1]) * cos(x[0]), sin(x[0]) * cos(x[1]), sin(x[1]) * cos(x[0])};
    };

    Func11 f = [](Real x)
    {
        return cos(x);
    };

    FSolve fs;
    Real t = fs(f, 1);
    cout << t << endl;
    cout << f(t) << endl;

    Jacobian j;
    Vector X({0, 0});
    // Matrix J = j(fX, X);
    // print(J);

    X[0] = 1;
    X[1] = 1;
    fs(fX, X);
    print(X);
    Vector Y = fX(X);
    print(Y);
}

void test8()
{
    deque<Real> interval({1, 2, 3});
    PiecewisePolynomial ppoly;
    ppoly.push_back(1);
    ppoly.push_back(2);
    ppoly.push_back(3);
    ppoly[0] = Polynomial({2, 1, 1});
    ppoly[1] = Polynomial({2, 1, 1});
    ppoly[2] = Polynomial({2, 3, 3});
    ppoly[3] = Polynomial({4, 5, 5});
    print(ppoly);

    cout << ppoly.continuity(0) << endl;

    // PiecewisePolynomial ppoly2;
    // ppoly2 = ppoly * 2;
    // print(ppoly2);

    // PiecewisePolynomial ppoly3 = ppoly - ppoly2;
    // print(ppoly3);
    // ppoly3 = ppoly + ppoly2;
    // print(ppoly3);
    // ppoly3 = ppoly * ppoly2;
    // print(ppoly3);

    // cout << ppoly(0.5) << endl;
    // cout << ppoly(1.5) << endl;
    // cout << ppoly(2.5) << endl;
    // cout << ppoly(3.5) << endl;

    // PiecewisePolynomial ppoly4 = ppoly.intergral();
    // print(ppoly4);
    // ppoly4 = ppoly.derivative();
    // print(ppoly4);
}

void print(point3f p)
{
    cout << "p: " << p.x << " " << p.y << " " << p.z << endl;
}

void print(std::deque<Real> &knot)
{
    for (int i = 0; i < knot.size(); i++)
        cout << knot[i] << " ";
    cout << endl;
}

void print(deque<point3f> points)
{
    for (int i = 0; i < points.size(); i++)
        print(points[i]);
}

void test9()
{
    Bezier B(4, 4);
    deque<point3f> pList = {{0, 0, 0},
                            {0, 1, 0},
                            {1, 1, 0},
                            {1, 0, 0},
                            {0, 0, 1},
                            {0, -1, 1},
                            {1, -1, 1},
                            {1, 0, 1},
                            {0, 0, 2},
                            {0, 1, 2},
                            {1, 1, 2},
                            {1, 0, 2},
                            {0, 0, 3},
                            {0, -1, 3},
                            {1, -1, 3},
                            {1, 0, 3}};
    B.setCtrlPoints(pList);
    point3f pt = B.calculateUV(0.8, 0.4);
    print(pt);
    Vector x = B.paramUV(pt);
    print(x);

    deque<Real> v = {1, 2, 3, 4, 5};
    PiecewisePolynomial pp = BasicBSpline(v);
    print(pp);

    deque<point3f> pList1 = {{0, 0, 0}, {0, 1, 0}, {1, 1, 0}, {2, 0.6, 0}, {3, 0, 0}};
    BSpline B1(5);
    B1.setCtrlPoints(pList1);
    B1.uniformUKnot(3, 0, 5);
    point3f p2 = B1.calculateU(3.68);
    print(p2);
    cout << B1.paramU(p2) << endl;
}

void test10()
{
    deque<point3f> points = {{0, 0, 0},
                             {1, 0, 0},
                             {0.6, 0.6, 0},
                             {0, 1, 0}};
    point3f p = {1, 0.4, 0};
    bool check = inHull2D(p, points);
    cout << check << endl;
}

BSpline spline(10, 8);

void test11()
{
    float x = -5;
    float y = 1;
    float z = 4;

    // 控制点顺序从左到右，从前到后
    std::deque<point3f> P;
    for (int i = 0; i < 8; i++)
    {
        for (int j = 0; j < 10; j++)
        {
            P.push_back({x, y, z});
            x += 1;
            y *= -1;
        }
        x = -5;
        z -= 1;
        y *= -1;
    }

    // 设置控制点和节点向量
    spline.setCtrlPoints(P);
    spline.uniformUKnot(3, 0, 7);
    spline.uniformVKnot(3, 0, 5);

    point3f p = spline.calculateUV(5.3, 1.6);
    print(p);
    Vector v = spline.paramUV(p);
    print(v);
    // point3f p = spline.calculateUV(2.3, 4.6);
}

HEMesh mesh;

void test12()
{
    srand(time(0));

    for (int i = 0; i < 100; i++)
    {
        float x = 10.0 * rand() / RAND_MAX;
        float y = 10.0 * rand() / RAND_MAX;
        float z = 10.0 * rand() / RAND_MAX;
        mesh.CreateVertex({x, y, z});
    }
    mesh.ConvexHull();
    cout << "Finished" << endl;
}

void test13()
{
    Matrix A(4, 4, {4, -2, 4, 2, -2, 10, -2, -7, 4, -2, 8, 4, 2, -7, 4, 7});

    LinearSolver solver;
    Matrix Q(4, 4);
    for (int i = 0; i < 4; i++)
        Q(i, i) = 1;

    Matrix Qs(4, 4);
    for (int i = 0; i < 4; i++)
        Qs(i, i) = 1;

    Matrix T = A.Tridiagonal(&Qs);
    Matrix AQ = A * Qs;
    Matrix QT = Qs * T;
    print(AQ);
    print(QT);

    // Matrix Q1 = solver.implicitQR(A, &Q);
    // Matrix Q2 = solver.implicitQR(A, &Qs, true);
    // print(Q1);
    // print(Q2);

    // Matrix AQ = A * Q;
    // Matrix QA = Q * Q1;
    // print(AQ);
    // print(QA);

    // AQ = A * Qs;
    // QA = Qs * Q2;
    // print(AQ);
    // print(QA);

    // Q = solver.passJacobi(A, 10);
    // print(A);
    // print(Q);
}

void test14()
{
    // Matrix A(6, 4, {4, -2, 4, -2, 10, -2, 4, -2, 8, 2, -7, 4, 1, -8, 9, 3, 2, 4, 0, -6, 10, 8, 1, 4});
    Matrix A(6, 4, {1.6585, 0.8979, -0.2121, -2.1781, 6.6192, 2.1841, -2.0816, -2.2572, 9.1714, 0.5503, -4.8928, 4.0891, -3.7319, -2.1438, 0.4880, 2.6400, 2.5901, 3.2697, 1.0615, -5.9551, 9.6185, 8.4721, 0.3138, 3.9710});
    print(A);
    Matrix U(6, 6);
    for (int i = 0; i < 6; i++)
        U(i, i) = 1;
    Matrix V(4, 4);
    for (int i = 0; i < 4; i++)
        V(i, i) = 1;

    LinearSolver solver;
    Matrix D = solver.SVD(A, &U, &V);
    print(D);
    print(U);
    print(V);

    Matrix UAV = U.transpose() * A * V;
    print(UAV);
}

void test15()
{
    Vector x = {1, 2, 3, 4, 5};
    Vector y = {1.8, 3.4, -1.5, 6.7, -5.8};
    Polynomial p = interpolate(x, y);
    for (int i = 0; i < 5; i++)
    {
        cout << p(x[i]) << endl;
        cout << NevilleAitken(x, y, x[i]) << endl;
    }
    cout << p(1.2) << " " << NevilleAitken(x, y, 1.2) << endl;
}

void test16()
{
    Vector x = {0, 1, 2};
    std::vector<Vector> y({{1.8}, {2.6, -1.5, 3.4}, {2}});
    Polynomial p = NewtonInterp(x, y);
    cout << p(0) << endl;
    cout << p(1) << endl;
    cout << p.derivative()(1) << endl;
    cout << p.derivative().derivative()(1) << endl;
    cout << p(2) << endl;
}

#define PI 3.1415926

double B_f(double x)
{
    // return 1 / (1 + 25 * x * x);
    return sin(PI * x);
}

void test17()
{
    int N = 20;
    Vector x(N);
    std::vector<Vector> f;
    f.push_back({B_f(-1), 50.0 / 26 / 26});
    x[0] = -1;

    for (int i = 1; i < N - 1; i++)
    {
        double y = -1 + 2.0 * i / (N - 1);
        x[i] = y;
        f.push_back({B_f(y)});
    }
    x[N - 1] = 1;
    f.push_back({B_f(1), -50.0 / 26 / 26});

    PiecewisePolynomial spline = ppFormSpline(x, f, COMPLETE_SPLINE);

    for (int i = 0; i < N; i++)
    {
        double y = -1 + 2.0 * i / (N - 1);
        cout << B_f(y) << endl;
        cout << spline(y) << endl;
    }
}

void test18()
{
    int N = 20;
    Vector x(N);
    std::vector<Vector> f;
    for (int i = 0; i < N; i++)
    {
        double y = -1 + 2.0 * i / (N - 1);
        x[i] = y;
        f.push_back({B_f(y)});
    }

    PiecewisePolynomial spline = ppFormSpline(x, f, PERIODIC_SPLINE);

    for (int i = 0; i < N; i++)
    {
        double y = -1 + 2.0 * i / (N - 1);
        cout << B_f(y) << endl;
        cout << spline(y) << endl;
    }
}

double C_f(double x)
{
    return 1 / (1 + x * x);
}

void test19()
{
    int N = 11;
    Vector x(N);
    std::vector<Vector> f;
    f.push_back({C_f(-5), 10.0 / 26 / 26});
    x[0] = -5;

    for (int i = 1; i < N - 1; i++)
    {
        double y = -5 + i;
        x[i] = y;
        f.push_back({C_f(y)});
    }
    f.push_back({C_f(5), -10.0 / 26 / 26});
    x[N - 1] = 5;

    PiecewisePolynomial spline = cardinalBSpline3(x, f);

    for (int i = 1; i < N - 1; i++)
    {
        double y = -5 + i;
        cout << C_f(y) << endl;
        cout << spline(y) << endl;
    }
}

void test20()
{
    int N = 10;
    Vector x(N + 2);
    Vector f(N + 2);
    f[0] = C_f(-5);
    x[0] = -5;

    for (int i = 0; i < N; i++)
    {
        double y = -9.0 / 2 + i;
        x[i + 1] = y;
        f[i + 1] = C_f(y);
    }
    f[N + 1] = C_f(5);
    x[N + 1] = 5;

    PiecewisePolynomial spline = cardinalBSpline2(x, f);

    cout << C_f(-5) << endl;
    cout << spline(-5) << endl;
    for (int i = 0; i < N; i++)
    {
        double y = -9.0 / 2 + i;
        cout << C_f(y) << endl;
        cout << spline(y) << endl;
    }
    cout << C_f(5) << endl;
    cout << spline(5) << endl;
}

void test21()
{
    Vector v({1, 2, 3});
    Matrix m(1, 4, {1, 2, 3, 4});
    cout << v << endl;
    cout << m << endl;
    cout << v * m << endl;

    SpMatrix spm(m);
    cout << spm << endl;

    SpMatrix spm1(v * m);
    cout << spm1 << endl;

    deque<Real> knot = {0, 1, 2, 3, 4};
    PiecewisePolynomial ppoly = BasicBSpline(knot);
    cout << ppoly << endl;
}

int main()
{
    // test();
    // test2();
    // test3();
    // test4();
    // test5();
    // test6();
    // test7();
    // test8();
    // test9();
    // test10();
    // test11();
    // test12();
    // test13();
    // test14();
    // test15();
    // test16();
    // test17();
    // test18();
    // test19();
    // test20();
    test21();
    cout << "A" << endl;

    return 0;
}